COMMENT ⊗   VALID 00009 PAGES
C REC  PAGE   DESCRIPTION
C00001 00001
C00002 00002	BEGIN "TEST7"
C00004 00003	REAL SUBR DSKEW (ITG E1,E2)		α DISTANCE BETWEEN SKEW LINES
C00006 00004	REAL SUBR CAMCOS (ITG V1,V2,C)		α CAMERA RAY COSINES
C00009 00005	SUBR MKCAMRS			α MAKE AND POSITION TWO CAMERAS
C00012 00006	SUBR RECOORD		α CHANGE COORDINATE FRAME OF REFERENCE
C00015 00007	SUBR MKFORKS(REAL OM1,OM2)
C00022 00008	SUBR SETFORKS (REAL W1,W2,W3)		α SET CAMERA FORKS TO GIVEN POSITION
C00023 00009	α MAIN EXECUTION
C00026 ENDMK
C⊗;
BEGIN "TEST7"
	REQUIRE "ABBREV[SYS,BGB]" SOURCE_FILE;
	REQUIRE "GEOMES.HDR[GEM,HE]" SOURCE_FILE;
	EXTERNAL SAFE ITG ARRAY DPYBUF[0:1];

α GLOBAL DECLARATIONS;
	ITG FORK1,FORK2;
	REAL α1,α2;
	ITG U1,U2,U3,U4,TET;
	ITG CIRCLE1,CIRCLE2;
	ITG E,V1,V2,V3,V4;		α VERTICES OF TETRAHEDRON;
	ITG C1,C2,WRLD;			α CAMERAS & WORLD;
	ITG I,F1,F2;			α CAMERA FRAMES;
	REAL RR1,SS1,RR2,SS2;		α CIRCLE SIZES;
	REAL ARRAY RAYCOS[1:6,1:2];	α FORK RAYS;
	REAL V1D3,V1D4,V2D3,V2D4;	α OFF P1-P2 DISTANCES;
	ITG EV3,EV4,EU3,EU4;		α CAMERA RAYSL;

	REAL WW1,WW2,WW3,D3,D4;
	REAL C1X,C1Y,C1Z;
	REAL C2X,C2Y,C2Z;

REAL SUBR DSKEW (ITG E1,E2);		α DISTANCE BETWEEN SKEW LINES;
BEGIN "DSKEW"
	ITG V1,V2,U1,U2;
	REAL A1,B1,C1,A2,B2,C2,D;
	REAL C11,C12,C13,C21,C22,C23;
	REAL P,Q,R;

	V1←NVT(E1); V2←PVT(E1);	U1←NVT(E2); U2←PVT(E2);
	A1 ← XWC(V2) - XWC(V1);	B1 ← YWC(V2) - YWC(V1);	C1 ← ZWC(V2) - ZWC(V1);
	A2 ← XWC(U2) - XWC(U1);	B2 ← YWC(U2) - YWC(U1);	C2 ← ZWC(U2) - ZWC(U1);

	C11 ← A1*A1 + B1*B1 + C1*C1;
	C22 ← A2*A2 + B2*B2 + C2*C2;
	C12 ←-A1*A2 - B1*B2 - C1*C2;
	C21 ← C12;

	C13 ← A1*(XWC(V1)-XWC(U1))+B1*(YWC(V1)-YWC(U1))+C1*(ZWC(V1)-ZWC(U1));
	C23 ← A2*(XWC(U1)-XWC(V1))+B2*(YWC(U1)-YWC(V1))+C2*(ZWC(U1)-ZWC(V1));
	R ← C11*C22 - C12*C21;

	IF ABS(R) ≤ 0.0001 THEN ⊂ P←C13/C11; Q←0; ⊃ ELSE
	⊂ P ←(C13*C22 - C12*C23)/R;Q ←(C11*C23 - C13*C21)/R;⊃;
	P←-P;Q←-Q;
	RETURN(SQRT(
		((XWC(U1)+Q*A2) - (XWC(V1)+P*A1))↑2 +
		((YWC(U1)+Q*B2) - (YWC(V1)+P*B1))↑2 +
		((ZWC(U1)+Q*C2) - (ZWC(V1)+P*C1))↑2 ));
END "DSKEW";
REAL SUBR CAMCOS (ITG V1,V2,C);		α CAMERA RAY COSINES;
BEGIN "CAMCOS"
	REAL X1,Y1,Z1,X2,Y2,Z2,R1,R2;
	X1 ← XWC(V1) - XWC(C);			X2 ← XWC(V2) - XWC(C);
	Y2 ← YWC(V2) - YWC(C);			Y1 ← YWC(V1) - YWC(C);
	Z1 ← ZWC(V1) - ZWC(C);			Z2 ← ZWC(V2) - ZWC(C);
	R1 ← SQRT(X1*X1 + Y1*Y1 + Z1*Z1);	R2 ← SQRT(X2*X2 + Y2*Y2 + Z2*Z2);
	RETURN((X1*X2+Y1*Y2+Z1*Z2)/(R1*R2));
END "CAMCOS";

SUBR TRIAD (ITG V,I,J);
BEGIN "TRIAD"
	REAL Cα,Sα,Cβ,CG,X,Y,Z;
	Cα ← RAYCOS[1,J]; Sα ← SQRT(1-Cα*Cα);
	Cβ ← RAYCOS[2+I,J];
	CG ← RAYCOS[4+I,J];
	X ← Cβ;		Y ← (CG-Cα*Cβ)/Sα;	Z ← SQRT(1-X*X-Y*Y);
	XWC(V)←X*10;	YWC(V)←Y*10;		ZWC(V)←Z*10;
END "TRIAD";
SUBR MKCAMRS;			α MAKE AND POSITION TWO CAMERAS;
BEGIN "MKCAMRS"
	ITG B1;

	PTOSTR(0,"βCβ}W↑Qα/α)\\**//εE");GEOMED;	α MAKE TWO CAMERAS;
	TET←INB3D("TET");GEODPY;		α TEST TETRAHEDRON;

	WRLD ← DAD(UNIVERSE);			α WORLD NODE;
	C1 ← DAD(WRLD); F1 ← ALT2(C1);		α CAMERA & FRAME ONE;
	C2 ← SON(WRLD); F2 ← ALT2(C2);		α CAMERA & FRAME TWO;
	V1←PVT(TET);V2←PVT(V1);			α VERTICES OF THE TETRAHEDRON;
	V3←PVT(V2);V4←PVT(V3);

	RAYCOS[1,1]←CAMCOS(V1,V2,F1);
	RAYCOS[2,1]←CAMCOS(V1,V3,F1);
	RAYCOS[3,1]←CAMCOS(V1,V4,F1);
	RAYCOS[4,1]←CAMCOS(V2,V3,F1);
	RAYCOS[5,1]←CAMCOS(V2,V4,F1);
	RAYCOS[6,1]←CAMCOS(V3,V4,F1);

	RAYCOS[1,2]←CAMCOS(V1,V2,F2);
	RAYCOS[2,2]←CAMCOS(V1,V3,F2);
	RAYCOS[3,2]←CAMCOS(V1,V4,F2);
	RAYCOS[4,2]←CAMCOS(V2,V3,F2);
	RAYCOS[5,2]←CAMCOS(V2,V4,F2);
	RAYCOS[6,2]←CAMCOS(V3,V4,F2);

α MAKE P1-P2 AXIS;
	B1 ← MKBFV; F1←PFACE(B1);
	V1 ← PVT(B1);     YWC(V1) ← +1;
	V1 ← MKEV(F1,V1); YWC(V1) ← -1;

α MAKE CIRCLES OF POTENTIAL CAMERA LOCI;
	α1 ← ACOS(RAYCOS[1,1]); α2 ← ACOS(RAYCOS[1,2]);
	RR1 ← 1/SIN(α1);	RR2 ← 1/SIN(α2);
	SS1 ← RR1*RAYCOS[1,1]; 	SS2 ← RR2*RAYCOS[1,2];

	CIRCLE1 ← MKCYLN(RR1,17,0);TRANSL(CIRCLE1,SS1,0,0);
	CIRCLE2 ← MKCYLN(RR2,17,0);TRANSL(CIRCLE2,SS2,0,0);
	OUTSTR("α1 = "&CVG(α1*180/π)&"α2 = "&CVG(α2*180/π)&↓);

END "MKCAMRS";
SUBR RECOORD;		α CHANGE COORDINATE FRAME OF REFERENCE;
BEGIN "RECOORD"
	REAL FACTOR,ARC,ORGX,ORGY,ORGZ;
	ITG WRLD,F1,F2,V1,V2,V3,V4;
	WRLD ← DAD(UNIVERSE);			α WORLD NODE;
	F1←ALT2(DAD(WRLD));F2←ALT2(SON(WRLD));	α CAMER FRAMES;
	V1←PVT(TET);V2←PVT(V1);			α VERTICES OF THE TETRAHEDRON;
	V3←PVT(V2);V4←PVT(V3);
α SCALE;
	FACTOR ← 2/DISTAN(V1,V2);
	SHRINK(TET,FACTOR,FACTOR,FACTOR);
	SHRINK(F1,FACTOR,FACTOR,FACTOR);
	SHRINK(F2,FACTOR,FACTOR,FACTOR);
α TRANSLATE;
	ORGX ← -0.5*(XWC(V1) + XWC(V2));
	ORGY ← -0.5*(YWC(V1) + YWC(V2));
	ORGZ ← -0.5*(ZWC(V1) + ZWC(V2));
	TRANSL(TET,ORGX,ORGY,ORGZ);
	TRANSL(F1,ORGX,ORGY,ORGZ);
	TRANSL(F2,ORGX,ORGY,ORGZ);
α ROTATE;
	ARC ← ATAN2(ZWC(V1),XWC(V1));
	ROTATE(TET,0,ARC,0);ROTATE(F1,0,ARC,0);ROTATE(F2,0,ARC,0);
	ARC ← ATAN2(XWC(V1),YWC(V1));
	ROTATE(TET,0,0,ARC);ROTATE(F1,0,0,ARC);ROTATE(F2,0,0,ARC);
	ARC ← ATAN2(ZWC(F1),XWC(F1));
	ROTATE(TET,0,ARC,0);ROTATE(F1,0,ARC,0);ROTATE(F2,0,ARC,0);
α SOLUTION CAMERA POSITIONS;
	XWC(V1)←ZWC(V1)←XWC(V2)←ZWC(V2)←ZWC(F1)←0;
	C1X ← XWC(F1);	C1Y ← YWC(F1);	C1Z ← ZWC(F1);
	C2X ← XWC(F2);	C2Y ← YWC(F2);	C2Z ← ZWC(F2);
	OUTSTR("CAMERA LOCUS 1: "&CVG(C1X)&CVG(C1Y)&CVG(C1Z)&↓);
	OUTSTR("CAMERA LOCUS 2: "&CVG(C2X)&CVG(C2Y)&CVG(C2Z)&↓);
END "RECOORD";
SUBR MKFORKS(REAL OM1,OM2);
BEGIN "MKFORKS"

α MAKE CAMERA RAY FORK1 IN ITS INITIAL POSITION;
	FORK2 ← MKBFV; F1←PFACE(FORK2);
	V3 ← PVT(FORK2);V1 ← MKEV(F1,V3);V4 ← MKEV(F1,V1);
	XWC(V3)←6; XWC(V4)←6*COS(α1);YWC(V4)←6*SIN(α1);U3←V3;U4←V4;U1←V1;
	
	FORK1 ← MKBFV; F1←PFACE(FORK1);BATT(FORK2,FORK1);
	V3 ← PVT(FORK1);V1 ← MKEV(F1,V3);V4 ← MKEV(F1,V1);
	TRIAD(V3,0,1); ZWC(V3)←-ZWC(V3);TRIAD(V4,1,1);

	ROTATE(FORK1,0,0,-OM1);
	XWC(V3)←XWC(V3)-RR1;XWC(V1)←XWC(V1)-RR1;XWC(V4)←XWC(V4)-RR1;
	XWC(U3)←XWC(U3)-RR1;XWC(U1)←XWC(U1)-RR1;XWC(U4)←XWC(U4)-RR1;
	TRANSL(FORK1,SS1,0,0);
	ROTATE(-FORK1,0,0,π);

	EV3←PED(V3);EV4←PED(V4);
	IF NVT(EV3)=V3 THEN INVERT(EV3);IF NVT(EV4)=V4 THEN INVERT(EV4);

α CAMERA CAMERA RAY FORK2 IN ITS INITIAL POSITION;
	FORK2 ← MKBFV; F1←PFACE(FORK2);
	U3 ← PVT(FORK2);U1 ← MKEV(F1,U3);U4 ← MKEV(F1,U1);
	TRIAD(U3,0,2);ZWC(U3)←-ZWC(U3);TRIAD(U4,1,2);

	ROTATE(FORK2,0,0,-OM2);
	XWC(U3)←XWC(U3)-RR2;XWC(U1)←XWC(U1)-RR2;XWC(U4)←XWC(U4)-RR2;
	TRANSL(FORK2,SS2,0,0);
	ROTATE(-FORK2,0,0,π);

	EU3←PED(U3);EU4←PED(U4);
	IF NVT(EU3)=U3 THEN INVERT(EU3);IF NVT(EU4)=U4 THEN INVERT(EU4);
END "MKFORKS";
SUBR SETFORKS (REAL W1,W2,W3);		α SET CAMERA FORKS TO GIVEN POSITION;
BEGIN "SETFORKS"
	REAL OM1,OM2;

	OM1 ← ATAN2(1-RR1*SIN(W1),SS1+RR1*COS(W1)) + W1;
	OM2 ← ATAN2(1-RR2*SIN(W2),SS2+RR2*COS(W2)) + W2;

	OUTSTR("OM1 = "&CVG(OM1*180/π)&"OM2 = "&CVG(OM2*180/π)&↓);

	IF FORK1≠0 THEN KLBFEV(FORK1);
	IF FORK2≠0 THEN KLBFEV(FORK2);
	MKFORKS(OM1,OM2);

	ROTATE(-FORK1,0,0,W1);
	ROTATE(-FORK2,0,0,W2);
	ROTATE( FORK2,0,W3,0);
END "SETFORKS";
α MAIN EXECUTION;
	MKCAMRS;
	RECOORD;

	WW1 ← ATAN2(C1Y,C1X-SS1);
	WW2 ← ASIN (C2Y/RR2);
	WW3 ← ATAN2(-C2Z,C2X);

	SETFORKS(WW1,WW2,WW3);
	D3 ← DSKEW(EV3,EU3);D4 ← DSKEW(EV4,EU4);
	OUTSTR(↓&"ERRORS: "&CVG(D3)&9&CVG(D4)&9&CVG(1/(1+D3+D4))&↓);
	GEOMED;

END "TEST7";